package org.hs.phd.odi.tracking.tracker.particlefilter.transition;

import java.util.Random;

import org.hs.phd.common.randomnumbers.GaussianRandomNumberGenerator;
import org.hs.phd.odi.tracking.tracker.particlefilter.core.Particle;
import org.hs.phd.odi.tracking.tracker.particlefilter.core.TransitionModel;
import org.hs.phd.odi.tracking.tracker.particlefilter.state.SphericalWindowParticleState;

public class SphericalWindowAutoRegressiveDynamicsTransitionModel implements TransitionModel<SphericalWindowParticleState> {

	/* autoregressive dynamics parameters for transition model */
	private static final float A1 = 2.0F;
	private static final float A2 = -1.0F;
	private static final float B0 = 1.0000F;
	
	private GaussianRandomNumberGenerator thetaNoise;
	private GaussianRandomNumberGenerator phiNoise;
	private GaussianRandomNumberGenerator sNoise;
	
	private SecondOrderAutoRegressiveTransitionHelper regression;

	/* random number generators for gaussian sampling in transition model */

	public SphericalWindowAutoRegressiveDynamicsTransitionModel() {
		this(new Random());
	}

	public SphericalWindowAutoRegressiveDynamicsTransitionModel(Random random) {
		thetaNoise = new GaussianRandomNumberGenerator(random, 0, 0.01F);
		phiNoise = new GaussianRandomNumberGenerator(random, 0, 0.01F);
		sNoise = new GaussianRandomNumberGenerator(random, 0, 0.001F);
		regression = new SecondOrderAutoRegressiveTransitionHelper(A1, A2, B0);
	}

	@Override
	public Particle<SphericalWindowParticleState> transition(Particle<SphericalWindowParticleState> p) {

		SphericalWindowParticleState currentState = p.getCurrentState();
		SphericalWindowParticleState initialState = p.initialState;
		SphericalWindowParticleState prevState = p.getPrevState(0);

		/* sample new state using second-order autoregressive dynamics */
		double theta = regression.transitionMinMaxTrim(initialState.theta, prevState.theta, currentState.theta, thetaNoise, Math.PI/4, Math.PI);
		double phi = regression.transition(initialState.phi, prevState.phi, currentState.phi, phiNoise);
		
		phi = (phi < 0 ) ? phi + 2*Math.PI : ((phi > 2*Math.PI ) ? phi - 2*Math.PI : phi);

		
		double s = regression.transitionMinMaxTrim(initialState.scale, prevState.scale, currentState.scale, sNoise, 0.1F, 10);

		
		SphericalWindowParticleState newState = currentState.createMovedScaledCopy(theta, phi, s);
		return p.createNextGenerationParticle(newState);
	}

}
